#include <Arduino.h>
#include <Wire.h>
#include <Config.h>
#include "cmd/cmd.h"
#include "FOC/FOC.h"
#include "BAT/BAT.h"
#include "FLASH_APP/FLASH_APP.h"
#include "BluetoothSerial.h"

#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif
// #include <SimpleFOC.h>

uint32_t chipId = 0;
BluetoothSerial SerialBT;

void focThread(void *pvParameters);
void otherThread(void *pvParameters);

void setup()
{
    Serial.begin(115200);
    SerialBT.begin("ESP32test"); // Bluetooth device name
    Serial.println("The device started, now you can pair it with bluetooth!");
    for (int i = 0; i < 17; i = i + 8)
    {
        chipId |= ((ESP.getEfuseMac() >> (40 - i)) & 0xff) << i;
    }
    Serial.printf("ESP32 Chip model = %s Rev %d MHz: %d\n", ESP.getChipModel(), ESP.getChipRevision(), ESP.getCpuFreqMHz());
    Serial.printf("This chip has %d cores\n", ESP.getChipCores());
    Serial.print("Chip ID: ");
    Serial.println(chipId);
    xTaskCreatePinnedToCore(
        focThread, "focThread" // A name just for humans
        ,
        4096 // This stack size can be checked & adjusted by reading the Stack Highwater
        ,
        NULL, 2 // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
        ,
        NULL, 1);
    xTaskCreatePinnedToCore(
        otherThread, "otherThread" // A name just for humans
        ,
        4096 // This stack size can be checked & adjusted by reading the Stack Highwater
        ,
        NULL, 2 // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
        ,
        NULL, 1);
    BAT::Init();
    CMD::init();
}

void loop()
{
    // delay(1);
}
void focThread(void *pvParameters)
{
    FOC::init();
    while (true)
    {
        FOC::Update();
        vTaskDelay(1);
    }
}
void otherThread(void *pvParameters)
{

    while (true)
    {
        BAT::getVol();
        if (Serial.available())
        {
            SerialBT.write(Serial.read());
        }
        if (SerialBT.available())
        {
            Serial.write(SerialBT.read());
        }
        delay(20);
    }
}